
#include "MCone.h"


MCone::MCone(float ar, float ah, int ars, int avs, int acn, float aox , float aoy ,float aoz )
{
	name = "Cone";
	verts = NULL;
	normal = NULL;
	bNormal = false;
	updateCone(ar,ah,ars,avs,acn,aox,aoy,aoz);
}


MCone::~MCone(void)
{
	if(verts) {
		for(int i = 0; i<rs; i++) {
			delete []verts[i];
		}
		delete []verts;
	}
	if(normal) {
		for(int i = 0; i<rs; i++) {
			delete normal[i];
		}
		delete []normal;
	}
}

void MCone::drawShape(void(*func)(const MVertex &p1,  const MVertex &p2, const MVertex &p3, int ic),
	void(*drawline)(const MVertex &p1,  const MVertex &p2,int ic)) {
	drawCone(func,drawline);
}

MCone *MCone::makeCone(float ar, float ah, int ars, int avs, int acn, float aox, float aoy,float aoz) {
	if(ar<=0  || ah <=0 || ars < 1 || avs < 3 || acn < 1) {//at least 2 radial stacks and 3 vertical slice
		return NULL;
	}
	return new MCone(ar, ah,ars,avs,acn,aox,aoy,aoz);
}

void MCone::updateCone(float ar, float ah, int ars, int avs, int acn, float aox, float aoy,float aoz) {
	if(ar<=0  || ah <=0 || ars < 1 || avs < 3 || acn < 1) {//at least 2 radial stacks and 3 vertical slice
		return;
	}
	//delete old array
	if(verts) {
		for(int i = 0; i<rs; i++) {
			delete []verts[i];
		}
		delete []verts;
	}
	if(normal) {
		for(int i = 0; i<rs; i++) {
			delete normal[i];
		}
		delete []normal;
	}
	ox = aox;
	oy = aoy;
	oz = aoz;
	r = ar;
	h = ah;
	rs = ars;
	vs = avs;
	cn = acn;
	step = 1;
	//create array
	verts = new MVertex*[rs];
	normal = new MVertex*[rs];
	int i, j;
	for(i = 0; i<rs; i++) {
		verts[i] = new MVertex[vs];
		normal[i] = new MVertex[vs];
	}
	//populate data
	MVertex p1(0,h,0);
	for(i=0; i<rs; i++) {
		p1 = p1.translate(0,-h/rs,0);
		float newr = (float)(i+1)/(float)rs*r;
		verts[i][0] = p1.translate(newr,0,0);
		for(j=1; j<vs; j++) {
			verts[i][j] = verts[i][j-1].rotateY(2*PI/vs);
		}
	}

	for(i=0; i<rs; i++) {
		for(j=0; j<vs; j++) {
			MVertex top = (i==0?MVertex(0,h,0):verts[i-1][j]);
			MVertex bottom = (i==(rs-1)?verts[i][j]:verts[i+1][j]);
			MVertex left = (j==0?verts[i][vs-1]:verts[i][j-1]);
			MVertex right = (j==(vs-1)?verts[i][0]:verts[i][j+1]);
			calculateNormal(&normal[i][j],verts[i][j],top,bottom,left,right);
		}
	}

	//update own axis
	v1 = MVertex();
	v2 = MVertex(0,h,0);

	translate(aox,aoz,aoy);
}

/*func is a function to draw triangles*/
void MCone::drawCone(void(*func)(const MVertex &p1,  const MVertex &p2, const MVertex &p3, int ic),
	void(*drawline)(const MVertex &p1,  const MVertex &p2,int ic)) {
	static const int icolor = 2;
	if(!func) {	//return function
		return;
	}
	int i, j;
	MVertex top(v2);
	for(j=0; j<vs; j++) {
		func(top,verts[0][j],verts[0][(j==vs-1?0:j+1)],icolor);
	}
	for(i=0; i<rs-1; i++) {
		for(j=0; j<vs; j++) {
			int nextj = ((j==vs-1)?0:j+1);
			func(verts[i][j],verts[i+1][j],verts[i+1][nextj], icolor);
			func(verts[i+1][nextj],verts[i][nextj], verts[i][j],icolor);
		}
	}

	if(bNormal && drawline) {
		for(i=0; i<rs; i++) {
			for(j=0; j<vs; j++) {
				drawline(verts[i][j],verts[i][j]+normal[i][j]*NORMALEN,1);
			}
		}
	}
	if(step < vs) {
		step++;
	}
}

/*
void MCone::resetStep() {
	step = 1;
}

bool MCone::isComplete() const {
	return step == vs;
}
*/
void MCone::incrementSlice() {
	if(vs < 50) {
		this->updateCone(r,h,rs,vs+1,cn,ox,oy,oz);
	}
}

void MCone::decrementSlice() {
	if(vs > 3) {
		this->updateCone(r,h,rs,vs-1,cn,ox,oy,oz);
	}
}

void MCone::incrementStack() {
	if(rs < 50) {
		this->updateCone(r,h,rs+1,vs,cn,ox,oy,oz);
	}
}

void MCone::decrementStack() {
	if(rs > 1) {
		this->updateCone(r,h,rs-1,vs,cn,ox,oy,oz);
	}
}

void MCone::translate(float x, float y, float z) {
	if(verts && !(isEqual(x,0) && isEqual(y,0) && isEqual(z,0))) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].translate(x,y,z);
			}
		}
		v1 = v1.translate(x,y,z);
		v2 = v2.translate(x,y,z);
	}
}

void MCone::rotate(float deg, int x, int y, int z) {
	float theta = deg/180*PI;
	if(x==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateX(theta);
				normal[i][j] = normal[i][j].rotateX(theta);
			}
		}
		v1 = v1.rotateX(theta);
		v2 = v2.rotateX(theta);
	}
	if(y==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateY(theta);
				normal[i][j] = normal[i][j].rotateY(theta);
			}
		}
		v1 = v1.rotateY(theta);
		v2 = v2.rotateY(theta);
	}
	if(z==1) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].rotateZ(theta);
				normal[i][j] = normal[i][j].rotateZ(theta);
			}
		}
		v1 = v1.rotateZ(theta);
		v2 = v2.rotateZ(theta);
	}
}

void MCone::scale(float x, float y, float z){
	if(verts) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = verts[i][j].scaling(x,y,z);
			}
		}
		v1 = v1.scaling(x,y,z);
		v2 = v2.scaling(x,y,z);
	}
}

void MCone::rotateAroundOwnAxis(float theta) {
	MMatrix mT, mM, mR, mMb, mTb;
	//set translate matrix
	mT.setMatrix(1,0,0,-v1.point[0],0,1,0,-v1.point[1],0,0,1,-v1.point[2],0,0,0,1);
	//set translate back matrix
	mTb.setMatrix(1,0,0,v1.point[0],0,1,0,v1.point[1],0,0,1,v1.point[2],0,0,0,1);
	//get its own world axis
	MVertex u = getVertex(v1,v2).normalize(); //u match to +x
	MVertex n,v;
	if(isEqual(u.point[0],0) && isEqual(u.point[2],0)) {//if u is the same as up vector
		if(isEqual(u.point[1],1)) {
			n = MVertex(1,0,0);
			v = MVertex(0,0,1);
		}
		else if(isEqual(u.point[1],-1)) {
			n = MVertex(0,0,1);
			v = MVertex(1,0,0);
		}
		else {
			fprintf(stderr,"rotateAroundOwnAxis error\n");
		}
	}
	else {
		n = u.CrossProduct(MVertex(0,1,0)).normalize(); //n match to +z
		v = n.CrossProduct(u).normalize(); //v match to +y
	}
	//set rotation matrix turn vertex from own axis to world axis
	mM.setMatrix(u.point[0], u.point[1], u.point[2], 0,
		v.point[0], v.point[1], v.point[2], 0,
		n.point[0], n.point[1], n.point[2],0,
		0,0,0,1);
	//set rotation matrix turn vertex from world axis to its own world
	mMb.setMatrix(u.point[0], v.point[0], n.point[0], 0,
		u.point[1], v.point[1], n.point[1], 0,
		u.point[2], v.point[2], n.point[2], 0,
		0,0,0,1);
	//set rotation matrix rotate vertex around x axis which is the central axis
	mR.setMatrix(1,0,0,0,
		0,cos(theta),-sin(theta),0,
		0,sin(theta),cos(theta),0,
		0,0,0,1);

	//now, get the final magic matrix
	//matrix = mTb * mMb * mR * mM * mT
	MMatrix normalRotation = mMb.dotMatrix(mR).dotMatrix(mM);
	matrix = mTb.dotMatrix(normalRotation).dotMatrix(mT);
	if(verts && normal) {
		for(int i=0; i<rs; i++) {
			for(int j=0; j<vs; j++) {
				verts[i][j] = matrix.dotVertex(verts[i][j]);
				normal[i][j] = normalRotation.dotVertex(normal[i][j]);
			}
		}
	}
}

void MCone::intersect(const MVertex &from, const MVertex &to) {
	MVertex p = from;
	MVertex d = (to - from).normalize();
	//inverse translate, rotate and scale
	p = p.translate(-mt.point[0], -mt.point[1], -mt.point[2]);
	p = p.rotateZ(-mr.point[2]);
	p = p.rotateY(-mr.point[1]);
	p = p.rotateX(-mr.point[0]);
	p = p.scaling(1/ms.point[0], 1/ms.point[1], 1/ms.point[2]);
	d = d.rotateZ(-mr.point[2]);
	d = d.rotateY(-mr.point[1]);
	d = d.rotateX(-mr.point[0]);
	d = d.scaling(1/ms.point[0], 1/ms.point[1], 1/ms.point[2]);
	d = d.normalize();
	list_intersect.clear();

	//formula for cone is x^2 + z^2 = ((h-y)*r/h)^2
	float a = d.x()*d.x() + d.z()*d.z() - pow(r/h*d.y(),2);
	float b = 2*(p.x()*d.x()+p.z()*d.z() + pow(r/h,2)*(h-p.y())*d.y());
	float c = p.x()*p.x() + p.z()*p.z() - pow(r/h*(h-p.y()),2);
	float f1 = b*b - 4*a*c;
	
	if(f1 > 0) { //two intersection points
		MVertex iter1 = p+d*((-b+sqrt(f1))/(2*a));
		MVertex iter2 = p+d*((-b-sqrt(f1))/(2*a));
		if(iter1.y()<=oy+h && iter1.y()>=oy) {
			list_intersect.push_back(iter1);
		}
		if(iter2.y()<=oy+h && iter2.y()>=oy) {
			list_intersect.push_back(iter2);
		}
	}else if(isEqual(f1,0)) {//only one intersection point
		MVertex iter1 = p+d*((-b+sqrt(f1))/(2*a));
		if(iter1.y()<=oy+h && iter1.y()>=oy) {
			list_intersect.push_back(iter1);
		}
	}else {//no intersection

	}

	//check bottom plane
	float t = -p.y()/d.y();
	float fx = p.x() + t*d.x();
	float fy = 0;
	float fz = p.z() + t*d.z();
	if(fx*fx+fz*fz <= r*r) {
		list_intersect.push_back(MVertex(fx,fy,fz));
	}
	//transit point back to world coordinate
	for(list<MVertex>::iterator it = list_intersect.begin(); it != list_intersect.end(); it++) {
		(*it) = (*it).scaling(ms.x(),ms.y(),ms.z());
		(*it) = (*it).rotateX(mr.x());
		(*it) = (*it).rotateY(mr.y());
		(*it) = (*it).rotateZ(mr.z());
		(*it) = (*it).translate(mt.x(),mt.y(),mt.z());
		cout<<"intersection: "<<(*it)<<endl;
	}
}